/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

// 
// This file test the command object and its functions
//
//

#include <stdlib.h>
#include <stdio.h>
#include <iostream>

//typedef unsigned char uchar8;
//typedef unsigned short UINT16;
//typedef unsigned int UINT32;

#include "command.h"

int main(int argc, char *argv[])
{

	cout << "sizeof(CommStruct)=" << sizeof(CommStruct) << endl;

	// make a new command and print it
	Command cmd(CMD_PACKET_SIZE + 3);
	cmd.cmd(8);
	cmd.num(3);
	uchar8 parms[] = {1,2,3};
	cmd.setParams(parms, cmd.num() );

	// test checksum
	cout << "!!!!!!!!!! Test command !!!!!!!!!!" << endl;
	cout << "before setting CheckSum: cmd.isValid=" << cmd.isValid() << endl;
	cmd.setCheckSum();
	cout << "after setCheckSum: cmd.isValid=" << cmd.isValid() << endl;
	cout << "is this defined: cmd.isDefined=" << cmd.isDefined() << endl;
	cout << cmd;

	// do some buffer tests
	uchar8 buf[]  = {  2, 3, 4, 5, 6, 7, 8};
	uchar8 buf1[] = {  0, 0, 0, 0, 0, 0, 0};

	// construct from buffer
	cout << "########## Constructor from a char buf ##########" << endl;
	Command cmd1(buf);
	cout << "!!!!!!!!!! Test command !!!!!!!!!!" << endl;
	cout << "before setting CheckSum: cmd.isValid=" << cmd1.isValid() << endl;
	cmd1.setCheckSum();
	cout << "after setCheckSum: cmd.isValid=" << cmd1.isValid() << endl;
	cout << "is this defined: cmd.isDefined=" << cmd1.isDefined() << endl;
	cmd1.printCmd();

	// copy from buffer
	cout << "########## Fill from a char buf ##########" << endl;
	Command cmd2;
	cmd2.copyFromBuf(buf);	
	cout << "!!!!!!!!!! Test command !!!!!!!!!!" << endl;
	cout << "before setting CheckSum: cmd.isValid=" << cmd2.isValid() << endl;
	cmd2.setCheckSum();
	cout << "after setCheckSum: cmd.isValid=" << cmd2.isValid() << endl;
	cout << "is this defined: cmd.isDefined=" << cmd2.isDefined() << endl;

	// copy to buffer
	cout << "########## Fill to a char buf ##########" << endl;
	cmd2.copyToBuf(buf1);	
	for(int i=0; i < cmd2.size(); i++)
		printf("\t0x%x", buf1[i]);
	cout << endl;
	
	// Equality
	cout << "cmd1 should equal cmd2: cmd1==cmd2 =" << (cmd1 == cmd2) << endl;
	cmd2.printCmd();

	// defined command
	{
		cout << "########## Checking Defined Commands ##########" << endl;
		uchar8 buf3[] = {CMD_CONNECTING, 0, 0, 0};
		Command cmd(buf3);	
		cmd.printCmd();
		cout << "!!!!!!!!!! Test command !!!!!!!!!!" << endl;
		cout << "before setting CheckSum: cmd.isValid=" << cmd.isValid() << endl;
		cmd.setCheckSum();
		cout << "after setCheckSum: cmd.isValid=" << cmd.isValid() << endl;
		cout << "is this defined: cmd.isDefined=" << cmd.isDefined() << endl;
	}


	// command queue
	cout << endl << "########## Command Queue ##########" << endl;
	CommandQueue queue;
	uchar8 fifo[] = {CMD_CONNECTING, 0, 0xf8, 0xff, 1 ,1, CMD_DISCONNECTING, 1, 0, 0xf6, 0xff};
	queue.push(fifo, 11);
	cout << "Queue length=" << queue.length() << endl;
	cout << "Next command isValid=" << queue.isValid() << endl;
	while( queue.length() >= CMD_PACKET_SIZE ) {
		if( queue.isValid() ) {
			Command cmd = queue.popCommand();
			cout << "!!!!!!!!!! Test command !!!!!!!!!!" << endl;
			cout << "is this valid: cmd.isValid=" << cmd.isValid() << endl;
			cout << "is this defined: cmd.isDefined=" << cmd.isDefined() << endl;
		} else {
			cout << "dropping byte" << endl; 
			(void)queue.pop();
		}
	}

	// print out defined command arraw
	cout << endl << "##############################" << endl;
	cout << "#     Defined Commands       #" << endl
		<< "#____________________________#" << endl
		<< "| name        | id |   num   |" << endl
		<< "------------------------------" << endl;
	int num;
	for( int i=0; i< CMD_MAX_NUM; i++) 
		if(  DefinedCommands[ i ].id == i ) {
			cout << DefinedCommands[ i ].name;
			if( strlen( DefinedCommands[ i ].name ) <= 7 )
				cout << "\t";
			cout << "\t" << (int)DefinedCommands[ i ].id;
			num = (int)DefinedCommands[ i ].numParams;
			if( num == 255 )
				cout << "\t" << "DYNAMIC" << endl;
			else
				cout << "\t" << num << endl;
		}
	cout << endl;

	return EXIT_SUCCESS;
}
